#include "LFOSCommon.h"
#include "LFOSC2Interface.h"
#include "EdgeDetection.h"
#include "LineExtraction.h"
#include <iostream>
#include <fstream>
#include <cmath>

using namespace std;

LFOSC2Interface::LFOSC2Interface()
{
	intersectionLocation.x = -1;
	intersectionLocation.y = -1;
}

DetectionInfo LFOSC2Interface::ProcessImage(unsigned char* image)
{
	DetectionInfo result;
	bool* edgeImage;
	//bool edgeImage[PIXELS];
	bool intAhead, reached, obstruction;
	LineSegment* segments;
	unsigned int numSegments;
	int angle, distance;
	Point edgePoints[PIXELS];
	int numPoints;

	result.type = 0;

	edgeImage = ed.ProcessEdges(image, edgePoints, numPoints);
//	for(int i = 0; i < PIXELS; i++)
//	{
//		if(image[i] == 255)
//			edgeImage[i] = true;
//		else
//			edgeImage[i] = false;
//	}
	segments = le.ExtractLines(edgeImage, numSegments, edgePoints, numPoints);

	obstruction = FindObstruction(segments, numSegments);
	reached = FindIntersection(segments, numSegments, intAhead);

	if(intAhead || reached)
	{
		if(reached)
		{
			result.type |= IntersectionReached;
		}
		else
		{
			result.type |= IntersectionAhead;
		}
		result.drivewayIntersection = drivewayIntersection;
		result.roadLeft = roadLeft;
		result.roadRight = roadRight;
		result.roadStraight = roadStraight;
		result.intersection.x = intersectionLocation.x;
		result.intersection.y = intersectionLocation.y;
	}


	if(obstruction && obstructionLocation.y > intersectionLocation.y)
		result.type |= Obstruction;

	if((result.type & IntersectionReached) == 0 &&
			(result.type & Obstruction) == 0)
	{
		FindClosestLine(segments, numSegments, angle, distance);
		result.type |= LineDetection;
		result.angle = angle;
		result.fromCenter = distance;
	}

	return result;
}

bool LFOSC2Interface::FindIntersection(LineSegment* segments, unsigned int numSegments,
		bool& ahead)
{
	Point lineIntersects[100];
	LineSegment line1[100];
	LineSegment line2[100];
	int numIntersects, clusters, idx;
	int t1, r1, t2, r2;
	Point intPoint;
	//Stores indices of intersections in same cluster
	int clusteredIntersections[100][100];

	int numInCluster[100];
	Point intersectionLocations[100];
	bool found, retVal;
	int bestCluster;
	Point newLocation;

	clusters = 0;
	numIntersects = 0;
	retVal = 0;

	//find all intersections
	for(unsigned int i = 0; i < numSegments && numIntersects < 100; i++)
	{
		t1 = (segments[i].theta - C2THETAMIN)/C2THETASTEP;
		r1 = segments[i].r;
		if(LineLengthSquare(segments[i]) <= 625 || i == obstructionSegmentIndex[0] ||
				i == obstructionSegmentIndex[1])
			continue;
		for(unsigned int j = i; j < numSegments && numIntersects < 100; j++)
		{
			t2 = (segments[j].theta - C2THETAMIN)/C2THETASTEP;
			r2 = segments[j].r;
			if(t1 != t2 && j != obstructionSegmentIndex[0] && j != obstructionSegmentIndex[1] &&
					LineLengthSquare(segments[j]) >= 625)
			{
				//find the intersection point of the lines
				intPoint.x = C2TRIGFACTOR*(r1*sin[t2]-r2*sin[t1])/(cos[t1]*sin[t2] - cos[t2]*sin[t1]);
				if(sin[t1] != 0)
					intPoint.y = r1*C2TRIGFACTOR/sin[t1] - intPoint.x*cos[t1]/sin[t1];
				else
					intPoint.y = r2*C2TRIGFACTOR/sin[t2] - intPoint.x*cos[t2]/sin[t2];

				if(intPoint.y < (ROWS - 8) && IsPointOnLineSegment(segments[i], intPoint) &&
						IsPointOnLineSegment(segments[j], intPoint))
				{
					lineIntersects[numIntersects].x = intPoint.x;
					lineIntersects[numIntersects].y = intPoint.y;
					line1[numIntersects] = segments[i];
					line2[numIntersects] = segments[j];
					numIntersects++;
				}
			}
		}
	}

	//Cluster intersection points
	for(int i = 0; i < numIntersects; i++)
	{
		//Loop through clusters and find closest one
		found = false;
		for(int j = 0; j < clusters; j++)
		{
			if(abs(lineIntersects[i].x - intersectionLocations[j].x) <= 75 &&
					abs(lineIntersects[i].y - intersectionLocations[j].y) <= 30)
			{
				found = true;
				idx = j;
				break;
			}
		}
		//if found add to cluster
		if(found)
		{
			clusteredIntersections[idx][numInCluster[idx]] = i;
			intersectionLocations[idx].x = (intersectionLocations[idx].x*numInCluster[idx] +
					lineIntersects[i].x)/(numInCluster[idx]+1);
			intersectionLocations[idx].y = (intersectionLocations[idx].y*numInCluster[idx] +
					lineIntersects[i].y)/(numInCluster[idx]+1);
			numInCluster[idx]++;
		}
		//not found - create new cluster
		else
		{
			clusteredIntersections[clusters][0] = i;
			numInCluster[clusters] = 1;
			intersectionLocations[clusters].x = lineIntersects[i].x;
			intersectionLocations[clusters].y = lineIntersects[i].y;
			clusters++;

		}
	}

	//Find cluster with max points
	int maxClusters = 0;
	bestCluster = -1;
	int minDist = 500;
	int dx,dy,dist;
	for(int i = 0; i < clusters; i++)
	{
		if(numInCluster[i] > maxClusters)
		{
			maxClusters = numInCluster[i];
			bestCluster = i;
			dx = intersectionLocations[i].x - intersectionLocation.x;
			dy = intersectionLocations[i].y - intersectionLocation.y;
			minDist = dx*dx+dy*dy;
		}
		else if(numInCluster[i] == maxClusters)
		{
			dx = intersectionLocations[i].x - intersectionLocation.x;
			dy = intersectionLocations[i].y - intersectionLocation.y;
			dist = dx*dx+dy*dy;
			if(dist < minDist)
			{
				maxClusters = numInCluster[i];
				bestCluster = i;
				minDist = dist;
			}
		}
	}

	bool intDriveway = false;
	bool right = false;
	bool left = false;
	bool straight = false;
	if(bestCluster != -1)
	{
		newLocation.x = intersectionLocations[bestCluster].x;
		newLocation.y = intersectionLocations[bestCluster].y;
		//Determine type of intersection and roads at intersection
		for(int i = 0; i < numInCluster[bestCluster]; i++)
		{
			idx = clusteredIntersections[bestCluster][i];
			if( ((line1[idx].theta - line2[idx].theta) > -75 &&
					(line1[idx].theta - line2[idx].theta) < -25) ||
					((line1[idx].theta - line2[idx].theta) > 25 &&
					(line1[idx].theta - line2[idx].theta) < 75) ||
					((line1[idx].theta - line2[idx].theta) > -165 &&
					(line1[idx].theta - line2[idx].theta) < -115) ||
					((line1[idx].theta - line2[idx].theta) > 115 &&
					(line1[idx].theta - line2[idx].theta) < 165)
			)
				intDriveway |= true;
//			else
//				intDriveway = false;

			//check for intersection greater than 45 degree intersection
			if((line1[idx].theta - line2[idx].theta) < -35 ||
					(line1[idx].theta - line2[idx].theta) > 35)
			{
				//Check for left road
				if(line1[idx].p1.x < line1[idx].p2.x)
				{
					left |= (LineLengthSquare(line1[idx]) >= 575 &&
							line1[idx].p1.x < (newLocation.x - 10));
				}
				else
				{
					left |= (LineLengthSquare(line1[idx]) >= 575 &&
							line1[idx].p2.x < (newLocation.x - 10));
				}
				if(line2[idx].p1.x < line2[idx].p2.x)
				{
					left |= (LineLengthSquare(line2[idx]) >= 575 &&
							line2[idx].p1.x < (newLocation.x - 10));
				}
				else if(line2[idx].p1.x >= line2[idx].p2.x)
				{
					left |= (LineLengthSquare(line2[idx]) >= 575 &&
							line2[idx].p2.x < (newLocation.x - 10));
				}

				//Check for right road
				if(line1[idx].p1.x < line1[idx].p2.x)
				{
					right |= (LineLengthSquare(line1[idx]) >= 575 &&
//							line1[idx].p2.x > (newLocation.x + 25));
							line1[idx].p2.x > (newLocation.x + 10));
				}
				else if(line1[idx].p1.x >= line1[idx].p2.x)
				{
					right |= (LineLengthSquare(line1[idx]) >= 575 &&
//							line1[idx].p1.x > (newLocation.x + 25));
							line1[idx].p1.x > (newLocation.x + 10));
				}
				if(line2[idx].p1.x < line2[idx].p2.x)
				{
					right |= (LineLengthSquare(line2[idx]) >= 575 &&
							line2[idx].p2.x > (newLocation.x + 10));
				}
				else if(line2[idx].p1.x >= line2[idx].p2.x)
				{
					right |= (LineLengthSquare(line2[idx]) >= 575 &&
							line2[idx].p1.x > (newLocation.x + 10));
				}

				//Check for straight road
				if( (line1[idx].theta <= 15  && line1[idx].theta >= -15) &&
						line1[idx].p1.y < line1[idx].p2.y)
				{
					straight |= (LineLengthSquare(line1[idx]) >= 575 &&
							line1[idx].p1.y < (newLocation.y - 10));
				}
				else if( (line1[idx].theta <= 15 && line1[idx].theta >= -15) &&
						line1[idx].p1.y >= line1[idx].p2.y)
				{
					straight |= (LineLengthSquare(line1[idx]) >= 575 &&
							line1[idx].p2.y < (newLocation.y - 10));
				}
				if( (line2[idx].theta <= 15  && line2[idx].theta >= -15) &&
						line2[idx].p1.y < line2[idx].p2.y)
				{
					straight |= (LineLengthSquare(line2[idx]) >= 575 &&
							line2[idx].p1.y < (newLocation.y - 10));
				}
				else if( (line2[idx].theta <= 15 && line2[idx].theta >= -15) &&
						line2[idx].p1.y >= line2[idx].p2.y)
				{
					straight |= (LineLengthSquare(line2[idx]) >= 575 &&
							line2[idx].p2.y < (newLocation.y - 10));
				}
			}
		}
	}

	if(!(straight || left || right))
	{
		newLocation.x = -1;
		newLocation.y = -1;
		ahead = false;
	}
	else
		ahead = true;

	if(newLocation.y < intersectionLocation.y)
	{
		retVal = true;
		ahead = false;
	}

	if(ahead)
	{
//		ahead = true;
		retVal = false;
		drivewayIntersection = intDriveway;
		roadLeft = left;
		roadRight = right;
		roadStraight = straight;
	}

	intersectionLocation.x = newLocation.x;
	intersectionLocation.y = newLocation.y;

	return retVal;
}

bool LFOSC2Interface::FindObstruction(LineSegment* segments, unsigned int numSegments)
{
	int r1, r2;
	int t1, t2;
	int intersectCount = 0;
	int totalIntersectCount = 0;
	int idx = 0;
	Point intPoint;
	Point location;

	obstructionSegmentIndex[0] = -1;
	obstructionSegmentIndex[1] = -1;

	//loop through each line segment and find all intersecting lines
	for(unsigned int i = 0; i < numSegments; i++)
	{
		intersectCount = 0;
		location.x = 0;
		location.y = 0;
		t1 = (segments[i].theta - C2THETAMIN)/C2THETASTEP;
		r1 = segments[i].r;
		for(unsigned int j = 0; j < numSegments && intersectCount < 2; j++)
		{
			t2 = (segments[j].theta - C2THETAMIN)/C2THETASTEP;
			r2 = segments[j].r;
			if(t1 != t2)
			{
				//find the intersection point of the lines
				intPoint.x = C2TRIGFACTOR*(r1*sin[t2]-r2*sin[t1])/(cos[t1]*sin[t2] - cos[t2]*sin[t1]);
				if(sin[t1] != 0)
					intPoint.y = r1*C2TRIGFACTOR/sin[t1] - intPoint.x*cos[t1]/sin[t1];
				else
					intPoint.y = r2*C2TRIGFACTOR/sin[t2] - intPoint.x*cos[t2]/sin[t2];
//				if(x > 0 && x < COLS && y > 0 && y < ROWS &&
//						IsPointOnLineSegment(segments[i], intPoint) &&
//						IsPointOnLineSegment(segments[j], intPoint) )
				if(intPoint.y < (ROWS - 8) &&
						IsPointOnLineSegment(segments[i], intPoint) &&
						IsPointOnLineSegment(segments[j], intPoint) &&
						(segments[i].theta - segments[j].theta < -75 ||
								segments[i].theta - segments[j].theta > 75))
				{
					intersectCount++;
					location.x += intPoint.x;
					location.y += intPoint.y;
				}

			}
		}
		if(intersectCount >= 2)
		{
			totalIntersectCount += 2;
			obstructionSegmentIndex[idx++] = i;
			obstructionLocation.x += location.x;
			obstructionLocation.y += location.y;

		}
	}
	if(totalIntersectCount >= 4)
	{
		obstructionLocation.x /= 4;
		obstructionLocation.y /= 4;
	}


	return (totalIntersectCount >= 4);
}

void LFOSC2Interface::FindClosestLine(LineSegment* segments, unsigned int numSegments,
			             int& angle, int& distance)
{
	int distSum= 0, thetaSum = 0;
	int numLines = 0;
	int t, x;

	//Fine lines close to bottom of image with angles between -45 and 45
	for(unsigned int i = 0; i < numSegments; i++)
	{
		if(segments[i].theta >= -45 && segments[i].theta <= 45 &&
				segments[i].p2.y > 200 && (segments[i].p2.y - segments[i].p1.y) > 35)
		{
			if(segments[i].theta == 0)
				x = segments[i].r;
			else
			{
				t = (segments[i].theta - C2THETAMIN)/C2THETASTEP;
				x = segments[i].r*C2TRIGFACTOR/cos[t] - (COLS-1)*sin[t]/cos[t];
			}
			distSum += ((COLS/2 - 1) - x);
			thetaSum += segments[i].theta;
			numLines++;
		}
	}

	if(numLines != 0)
	{
		cout << thetaSum << "   " << distSum << endl;
		angle = thetaSum/numLines;
		distance = distSum/numLines;
	}
	else
	{
		angle = 0;
		distance = 0;
	}
}

bool LFOSC2Interface::IsPointOnLineSegment(LineSegment ls, Point p)
{
	bool result;

	if(ls.p1.x < ls.p2.x)
	{
		result = (p.x >= (ls.p1.x - 3) && p.x <= (ls.p2.x + 3));
	}
	else
	{
		result = (p.x >= (ls.p2.x - 3) && p.x <= (ls.p1.x + 3));
	}

	if(result && ls.p1.y < ls.p2.y)
	{
		result &= (p.y >= (ls.p1.y - 3) && p.y <= (ls.p2.y + 3));
	}
	else if(result)
	{
		result &= (p.y >= (ls.p2.y - 3) && p.y <= (ls.p1.y + 3));
	}

	return result;
}

int LFOSC2Interface::LineLengthSquare(const LineSegment& ls)
{
	int dx, dy;

	dx = ls.p1.x - ls.p2.x;
	dy = ls.p1.y - ls.p2.y;

	return dx*dx + dy*dy;
}


//short LFOSC2Interface::cos[THETASIZE] = {70, 139, 208, 276, 342, 407, 469, 530, 588, 643, 695, 743, 788, 829, 866, 899, 927, 951,
//		970, 985, 995, 999, 999, 995, 985, 970, 951, 927, 899, 866, 829, 788, 743, 695, 643, 588, 530, 469, 407, 342, 276, 208,
//		139, 70, 0};
//
//short LFOSC2Interface::sin[THETASIZE] = {-998, -990, -978, -961, -940, -914, -883, -848, -809, -766, -719, -669, -616, -559, -500,
//		-438, -375, -309, -242, -174, -105, -35, 35, 105, 174, 242, 309, 375, 438, 500, 559, 616, 669, 719, 766, 809, 848, 883,
//		914, 940, 961, 978, 990, 998, 1000};

short LFOSC2Interface::cos[C2THETASIZE] = {35, 70, 105, 139, 174, 208, 242, 276, 309, 342, 375, 407, 438, 469, 500, 530, 559, 588,
		616, 643, 669, 695, 719, 743, 766, 788, 809, 829, 848, 866, 883, 899, 914, 927, 940, 951, 961, 970, 978, 985, 990, 995,
		998, 999, 1000, 999, 998, 995, 990, 985, 978, 970, 961, 951, 940, 927, 914, 899, 883, 866, 848, 829, 809, 788, 766, 743,
		719, 695, 669, 643, 616, 588, 559, 530, 500, 469, 438, 407, 375, 342, 309, 276, 242, 208, 174, 139, 105, 70, 35, 0};

short LFOSC2Interface::sin[C2THETASIZE] = {-999, -998, -995, -990, -985, -978, -970, -961, -951, -940, -927, -914, -899, -883, -866,
		-848, -829, -809, -788, -766, -743, -719, -695, -669, -643, -616, -588, -559, -530, -500, -469, -438, -407, -375, -342, -309,
		-276, -242, -208, -174, -139, -105, -70, -35, 0, 35, 70, 105, 139, 174, 208, 242, 276, 309, 342, 375, 407, 438, 469, 500, 530,
		559, 588, 616, 643, 669, 695, 719, 743, 766, 788, 809, 829, 848, 866, 883, 899, 914, 927, 940, 951, 961, 970, 978, 985, 990,
		995, 998, 999, 1000};
